#include "Comprocess.h"
#include "config.h"
#include<fstream>
#include<string>
#include<cmath>
Comprocess::Comprocess()
{
}


Comprocess::~Comprocess()
{
}

int Comprocess::executebin2pcd(char *source_dir, const string pcd_output_path) {
	listFiles(source_dir);
	for (int i = 0; i < filelist.size(); i++) {
		bin2pcd(source_dir, filelist[i], pcd_output_path);
	}
	return 1;
}
vector<string> Comprocess::listFiles(const char * dir)
{
	char dirNew[200];
	strcpy(dirNew, dir);
	strcat(dirNew, "\\*.*");    // 

	intptr_t handle;
	_finddata_t findData;

	handle = _findfirst(dirNew, &findData);
	if (handle == -1)        // 
		return filelist;

	do
	{
		if (findData.attrib & _A_SUBDIR)
		{
			if (strcmp(findData.name, ".") == 0 || strcmp(findData.name, "..") == 0)
				continue;

			cout << findData.name << "\t<dir>\n";

			// 
			strcpy(dirNew, dir);
			strcat(dirNew, "\\");
			strcat(dirNew, findData.name);

			listFiles(dirNew);
		}
		else {
			//cout << findData.name << "\t" << findData.size << " bytes.\n";
			filelist.push_back(findData.name);
		}
	} while (_findnext(handle, &findData) == 0);

	_findclose(handle);    // 
	return filelist;
}

void Comprocess::bin2pcd(const char * dir, const string filename,const string pcd_output_path) {
	string inputfilename = filename;
	inputfilename = inputfilename.insert(0, dir);
	const char* inptfile = inputfilename.data();
	int num = 1000000*100;
	float *data = (float*)malloc(num * sizeof(float));
	// 
	float *px = data + 0;
	float *py = data + 1;
	float *pz = data + 2;
	float *pr = data + 3;//
	// 
	FILE *stream;
	fopen_s(&stream, inptfile, "rb");
	num = fread(data, sizeof(float), num, stream) / 4;//
	fclose(stream);
	int after_erase_num = 0;
	double distance_threshold=1000000.0;
	float *px_t = px;
	float *py_t = py;
	float *pz_t = pz;
	float *pr_t = pr;//
	for (int i = 0; i < num; i++)
	{	
		//setting a threshold according to the distance between the point and centre to decrease the points 
		double distance = sqrt((*px_t)*(*px_t) + (*py_t)*(*py_t) + (*pz_t)*(*pz_t));
		if (distance < distance_threshold) {
			after_erase_num += 1;
		}
		px_t += 4; py_t += 4; pz_t += 4; pr_t += 4;

	}
	const char* outfile;
	string outputfilename = filename;
	outputfilename = outputfilename.replace(outputfilename.find(".") + 1, 3, "pcd");
	outfile = outputfilename.insert(0, pcd_output_path).data();
	FILE *writePCDStream;
	fopen_s(&writePCDStream, outfile, "wb");
	fprintf(writePCDStream, "VERSION 0.7\n");//
	fprintf(writePCDStream, "FIELDS x y z\n");//
	fprintf(writePCDStream, "SIZE 4 4 4\n");//
	fprintf(writePCDStream, "TYPE F F F\n");//
	fprintf(writePCDStream, "WIDTH %d\n", after_erase_num);//
	fprintf(writePCDStream, "HEIGHT 1\n");//
	fprintf(writePCDStream, "POINTS %d\n", after_erase_num);//
	fprintf(writePCDStream, "DATA ascii\n");//
	
	for (int32_t i = 0; i < num; i++)
	{
		double distance = sqrt((*px)*(*px) + (*py)*(*py) + (*pz)*(*pz));
		if (distance < distance_threshold) {
			fprintf(writePCDStream, "%f %f %f %f\n", *px, *py, *pz, *pr);
		}
		px += 4; py += 4; pz += 4; pr += 4;	
	}
	fclose(writePCDStream);
}

void Comprocess::transfromMatrix_Reader(vector<Eigen::Matrix4d> &transform,const string pose_data_path) {
	string file_path = pose_data_path;
	ifstream FileIn(file_path);
	Eigen::Matrix4d pose_data2;
	Eigen::Matrix4d velo2cam, cam2velo;
	cam2velo << 0, 0, 1, 0,
				-1, 0, 0, 0,
				0, -1, 0, 0.08,
				0, 0, 0, 1;
	velo2cam << 0, -1, 0, 0,
				0, 0, -1, 0,
				1, 0, 0, -0.08,
				0, 0, 0, 1;
	
	while (FileIn >> pose_data2(0,0) >> pose_data2(0, 1) >> pose_data2(0, 2) >> pose_data2(0, 3)
		>> pose_data2(1, 0) >> pose_data2(1, 1) >> pose_data2(1, 2) >> pose_data2(1, 3)
		>> pose_data2(2, 0) >> pose_data2(2, 1) >> pose_data2(2, 2) >> pose_data2(2, 3))
	{
		pose_data2(3, 0) = 0;
		pose_data2(3, 1) = 0;
		pose_data2(3, 2) = 0;
		pose_data2(3, 3) = 1;
		pose_data2 = cam2velo * pose_data2 * velo2cam;
		transform.push_back(pose_data2);
	}
	FileIn.close();
}

